% OUT:
% 1 - robot pomyslnie przemieszczony na kolejne pole
% 0 - przemieszczenie robota nie bylo mozliwe
%
% zalozenie:
% - robot nie przemieszcza sie na ukos (aktualna pozycja i kolejne pole w
% tablicy robot.droga sa sasiednimi polami)
function ret=wykonaj_ruch(indeks)
    global roboty;
    global stan_robotow;
    global czas;
    global mapa;
    global rozmiar_robota;
    global rozmiar_pola;
    
    robot = roboty(indeks);
    ret=0;
    
    %jesli sciezka jest niezerowa to sprobuj wykonac ruch
    if (~isequal(robot.droga(1,:),[0 0]))
        
        pozycja = robot.pozycja;
        kolejne_pole = robot.droga(1,:);

        % okreslenie wektora przesuniecia
        wektor=kolejne_pole-pozycja;
        roboty(indeks).kierunek = kierunek(wektor);
        
        % jesli robot jedzie wzdluz osi X to jego pozycja na osi Y na danym
        % polu powinna dazyc do wycentrowania
        if(wektor(1)~=0)
            wektor(2) = wektor(2)-sign(robot.xy(2)-rozmiar_pola/2);
        elseif(wektor(2)~=0) % analogicznie jak przy osi X
            wektor(1) = wektor(1)-sign(robot.xy(1)-rozmiar_pola/2);
        end;

        if(isequal(wektor,[0 0]))
            roboty(indeks).droga = robot.droga(2:size(robot.droga,1),:);
        end;
        
        % ruch na polu [11 1] do zajezdni
        if(robot.podstan==1)
            wektor=[0 0];
            if(robot.xy(2)>1) wektor = [0 -1]; end;
            if(robot.xy(1)<rozmiar_pola/2) wektor(1)=1;
            elseif(robot.xy(1)>rozmiar_pola/2) wektor(1)=-1; end;
        end;
        
        % wyliczenie nowych wspolrzednych lokalnych na aktualnym polu
        nowe_xy = robot.xy + wektor;
        
        % wyznaczenie sasiednich pol, ktore powinny zostac zarezerwowane
        % lub odrezerwowane przez robota
        wektor_rez = [0 0];
        wektor_odrez = [0 0];
        if(nowe_xy(1)-rozmiar_robota<1)
            wektor_rez(1)=-1;
        else
            if(robot.xy(1)-rozmiar_robota<1) wektor_odrez(1)=-1; end;
        end;
        if(nowe_xy(1)+rozmiar_robota>rozmiar_pola)
            wektor_rez(1)=1;
        else
            if(robot.xy(1)+rozmiar_robota>rozmiar_pola) wektor_odrez(1)=1; end;
        end;
        if(nowe_xy(2)-rozmiar_robota<1)
            wektor_rez(2)=-1;
        else
            if(robot.xy(2)-rozmiar_robota<1) wektor_odrez(2)=-1; end;
        end;
        if(nowe_xy(2)+rozmiar_robota>rozmiar_pola)
            wektor_rez(2)=1;
        else
            if(robot.xy(2)+rozmiar_robota>rozmiar_pola) wektor_odrez(2)=1; end;
        end;
        
        do_rezerwacji=[];
        if(~isequal(wektor_rez,[0 0]))
            tmp=pozycja+wektor_rez;
            if(tmp(1)>0 && tmp(2)>0 && tmp(1)<23 && tmp(2)<23)
                do_rezerwacji = [ tmp ];
            end;
            if(wektor_rez(1)~=0 && wektor_rez(2)~=0) 
                tmp=pozycja+[wektor_rez(1) 0];
                if(tmp(1)>0 && tmp(2)>0 && tmp(1)<23 && tmp(2)<23)
                    do_rezerwacji = [ do_rezerwacji; tmp ]; 
                end;
                tmp=pozycja+[0 wektor_rez(2)];
                if(tmp(1)>0 && tmp(2)>0 && tmp(1)<23 && tmp(2)<23)
                    do_rezerwacji = [ do_rezerwacji; tmp ]; 
                end;
            end;
        end;
        do_odrezerwowania=[];
        if(~isequal(wektor_odrez,[0 0]) && robot.podstan~=2 && robot.podstan~=3)
            tmp=pozycja+wektor_odrez;
            if(tmp(1)>0 && tmp(2)>0 && tmp(1)<23 && tmp(2)<23)
                do_odrezerwowania = [ tmp ];
            end;
            if(wektor_odrez(1)~=0) 
                tmp=pozycja+[wektor_odrez(1) 0];
                if(tmp(1)>0 && tmp(2)>0 && tmp(1)<23 && tmp(2)<23)
                    do_odrezerwowania = [ do_odrezerwowania; tmp ]; 
                end;
            end;
            if(wektor_odrez(2)~=0) 
                tmp=pozycja+[0 wektor_odrez(2)];
                if(tmp(1)>0 && tmp(2)>0 && tmp(1)<23 && tmp(2)<23)
                    do_odrezerwowania = [ do_odrezerwowania; tmp ]; 
                end;
            end;
        end;
        
        % robot dojechal on do regalu i powinien pobrac produkt
        if(abs(rozmiar_pola/2-nowe_xy(2))>=rozmiar_robota && robot.podstan==3)
            do_rezerwacji=[];
            %nowe_xy=robot.xy;
            roboty(indeks).podstan=0;
            roboty(indeks).stan=5; % pobieranie produktu
        end;
        
        % wyliczenie nowego pola zajmowanego przez robota
        nowe_pole=pozycja;
        if(nowe_xy(1)<1)
            nowe_xy(1)=rozmiar_pola;
            nowe_pole = nowe_pole + [-1 0];
        elseif(nowe_xy(1)>rozmiar_pola)
            nowe_xy(1)=1;
            nowe_pole = nowe_pole + [1 0];
        end;
        if(nowe_xy(2)<1)
            nowe_xy(2)=rozmiar_pola;
            nowe_pole = nowe_pole + [0 -1];
        elseif(nowe_xy(2)>rozmiar_pola)
            nowe_xy(2)=1;
            nowe_pole = nowe_pole + [0 1];
        end;
        
        % jesli robot wjechal do korytarza i usunal rezerwacje z pola przed
        % korytarzem, to nalezy zmienic podstan na 2, aby podczas dalszego
        % ruchu w korytarzu nie usuwal rezerwacji z poprzednich pol
        %if(robot.podstan==3 && size(do_odrezerwowania,2)~=0)
        %    roboty(indeks).podstan=2;
        %end;
        
        % do_rezerwacji==0 oznacza, ze robot po wykonaniu przesuniecia nie
        % zajmie swym rozmiarem zadnego nowego pola
        if(size(do_rezerwacji,2)==0)
            roboty(indeks).xy = nowe_xy;
            roboty(indeks).pozycja = nowe_pole;
            
            rezerwuj(0,indeks,do_odrezerwowania);
        else
        
            % proba rezerwacji pol
            if(rezerwuj(1,indeks,do_rezerwacji)==1) % zarezerwowane pomyslnie
                roboty(indeks).cierpliwosc = 3*rozmiar_pola;

                roboty(indeks).xy = nowe_xy;

                if(~isequal(robot.pozycja,nowe_pole))
                    roboty(indeks).pozycja=nowe_pole;

                    %teraz zmieniamy wsporzedne tablicy poniewaz posunelismy sie o kawalek
                    max=size(robot.droga,1);
                    roboty(indeks).droga = [robot.droga(2:max,:) ; 0 0];

                    % jesli robot porusza sie w glownym korytarzu i jest w
                    % poblizu swojego celu, powinien rozpoczac kolejkowanie,
                    % aby uniknac dojechania do samego celu (wjazdu korytarza/kasy),
                    % a potem wycofania, aby pojechac do kolejki
                    if(robot.stan==1 && size(robot.cel,2)==2 && abs(roboty(indeks).pozycja(1)-roboty(indeks).cel(1))+abs(roboty(indeks).pozycja(2)-roboty(indeks).cel(2))==1 && roboty(indeks).numer_kasy>0)
                        rozpocznij_kolejkowanie(indeks);
                    end;
                end;
                
                rezerwuj(0,indeks,do_odrezerwowania);

            else % pole niedostepne

                % manewrowanie, tylko jesli robot jest w magazynie
                if(~isequal(robot.pozycja,[0 0]))
                    % rozpocznij kolejkowanie, jesli pole jest zajete przez robota
                    % oczekujacego do tego samego korytarza
                    indeks2=mapa(robot.droga(1,1),robot.droga(1,2));

                    if(indeks2~=0 && indeks2~=indeks && ((roboty(indeks2).stan==4 && roboty(indeks2).indeks_korytarza==robot.indeks_korytarza && robot.indeks_korytarza~=0) || ((roboty(indeks2).stan==4 || roboty(indeks2).stan==6) && roboty(indeks2).numer_kasy==robot.numer_kasy && robot.numer_kasy~=0 && roboty(indeks2).indeks_korytarza==0))) %kolejkowanie
                            rozpocznij_kolejkowanie(indeks);
                    elseif(roboty(indeks).cierpliwosc>0)
                        indeks_poprzedniego=mapa(robot.droga(1,1),robot.droga(1,2));
                        % jesli poprzedni robot wykonuje kolejkowanie, to nie
                        % czekaj, az sie poruszy
                        if(roboty(indeks_poprzedniego).stan==4)
                            roboty(indeks).cierpliwosc = 0;
                        else
                            roboty(indeks).cierpliwosc = roboty(indeks).cierpliwosc - 1;
                        end;
                    else % podjecie innego dzialania
                        roboty(indeks).cierpliwosc = 3*rozmiar_pola;

                        % sprobuj wykonac manewr wyprzedzania
                        if(rozpocznij_manewr_wyprzedzania(indeks)==0) % niepowodzenie
                            %dijkstry(roboty(indeks).pozycja,roboty(indeks).cel,indeks,roboty(indeks).droga(1,:));
                        end;
                    end;
                end;
            end;
        end;
    else 
        % zerowa sciezka oznacza zakonczenie aktualnego dzialania i
        % koniecznosc przejscia do kolejnego stanu
        
        if(robot.stan==1) % ruch w glownym korytarzu
            
            if(isequal(robot.pozycja,[0 0]))
                if(rezerwuj(1,indeks,[11 1]))
                    % robot wyjezdza z zajezdni
                    roboty(indeks).pozycja=[11 1];
                    roboty(indeks).xy=[rozmiar_pola/2, 1];
                    roboty(indeks).kierunek=3;
                end;
                roboty(indeks).stan=0;
            elseif(isequal(robot.pozycja,[11 1]) && robot.numer_kasy==0)
                if(isequal(robot.xy,[rozmiar_pola/2, 1]))
                    % robot wjezdza do zajezdni
                    roboty(indeks).stan=0;
                    roboty(indeks).pozycja=[0 0];
                    roboty(indeks).xy=[0 0];
                    roboty(indeks).podstan=0;
                    rezerwuj(0,indeks,[11 1]);
                else
                    roboty(indeks).podstan=1;
                    roboty(indeks).droga=[11 1];
                end;
            elseif(size(roboty(indeks).produkty,2)>0)
                % oznacza, ze robot dojechal do docelowego korytarza i moze
                % rozpoczac manewr kolejkowania
                rozpocznij_kolejkowanie(indeks);
            else
                roboty(indeks).stan = 6; % rozladunek
            end;
            
        elseif(robot.stan==3) % wyprzedzanie
            
            roboty(indeks).stan=0; % stan 0 wymusi przeliczenie nowej drogi
            
        end;
        
    end;
